﻿/*****************************************************************//**
 * \file   SurgicalRobotPlatform.h
 * \brief  这个文件是SurgicalRobotPlatform.cpp的头文件，处理微创手术机器人实验平台主控界面的代码
 *         包括主控界面的按键处理和相机图像显示
 *
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021

 *********************************************************************/
#pragma once

#include <QMainWindow>
#include "ui_SurgicalRobotPlatform.h"
#include <QtConcurrent/QtConcurrentRun>
#include "SurgicalRobotConfig.h"
#include "SurgicalRobotRCM.h"
#include "SurgicalRobotCamera.h"
#include "SurgicalHapticControlRobotThread.h"
#include "SurgicalHapticThread.h"
#include "Eigen/Dense"
#include <QDateTime>
#include "log_mes.h"
#include "./hardware_interface/Motor_API/function.h"
#include "./hardware_interface/Motor_API/gary_ethercat/igh_preempt/ThreadObject.h"
#include "thread_test.h"


class SurgicalRobotPlatform : public QMainWindow
{
    Q_OBJECT

public:
	SurgicalRobotPlatform(QWidget* parent = Q_NULLPTR);
	~SurgicalRobotPlatform();

	void SetIndicatorColor(QRadioButton* radioButton, bool connect);
	void closeEvent(QCloseEvent* event);

public:
    int Motor_status = 0;
	int timerSurgicalID;
	int timerSurgicalControlRobotID;
	int timerReadRobotDataID;
	int timerCutfusionID;
    //ETHERCAT主站线程变量
    ThreadObject* m_obj;//real class object
    QThread* m_objThread;

	int timerTestFoceID;
	bool bControlStart;
	bool bVirtualFixturesOpen;
	bool bCutFusionOpen;
	QFuture<void> threadCommunicationFuture;
	QThread cameraThread;
	//QThread readRobotRunDataThread;
	SurgicalRobotCamera* surgicalCamera = nullptr;
	SurgicalHapticControlRobotThread* HapticControlRobotThread = nullptr;
    SurgicalHapticThread *HapticDataThread = nullptr;
    SurgicalRobotCamera *cameraProc = nullptr;
	SurgicalRobotCommunication* surgicalRobotCom = SurgicalRobotCommunication::GetInstance();
public:
	//alg part
	Eigen::Vector3d v3dRCMposition;
	Eigen::Matrix3d m3dRotationSimga2robot;
	Eigen::Matrix4d m4dTooltranM, m4dTooltranM0;
	Eigen::Matrix4d m4dTransM1, m4dTransM2, m4dTransM3, m4dTansMatrix;
	double dInstrumentLength = 0.3594;//0.3558; //手术器械的长度
	double dRCMlength = 0.15; //RCM点到手术器械杆最上端的距离（手术器械在人体外的长度）
	Eigen::Matrix4d m4dRobotStartposition[2], m4dHapticStartposition[2], m4dToolStartposition[2];
	Eigen::Quaterniond q1, q2;


	//实验算法数据
	//计算实验一所用的计时器
	std::chrono::time_point<std::chrono::system_clock> experiment01Start;
	std::chrono::time_point<std::chrono::system_clock> experiment01End;
	std::chrono::duration<double> experiment01Spend;

public:
	//实验数据保存
	std::fstream g_distance_reord, fr_distance_reord, time_experimrnt;
	std::fstream toolend_record, GVF_force_record, FRVF_force_record, Xiao_record1, Xiao_record2, Xiao_record3, toolend_recordR, toolend_recordT;

    //调用电机函数
    void get_motor_status();
    void motion_command(QString command);
signals:
    void show_message1(QString mess);
private slots:
	void RobotConfig();
	void RobotStart();
	void RobotRCMConfig();
	void RobotMSOStep();
	void VirtualDeviceStart();
	void VirtualDeviceStop();
	void CutFusionStart();
	void CutFusionStop();
	void RobotShutDown();
	void OpenCamera();
	void updateImage(QImage);
	void updateImage();
	void menuBarProcess();
	void onActionSaveDataPostionSet();
    void onTimer() {
        // 这是原来 OnTimer 函数的内容
        qDebug() << "alarm!";
        sem_post(&hHapticSemaphore);  // 释放信号量

        // 如果需要重复触发，可以在这里重新启动定时器
    };




    void on_btVirtualDeviceStart_triggered(QAction *arg1);

    void on_btVirtualDeviceStart_clicked();

    void on_btVirtualDeviceStop_clicked();


protected:
	void timerEvent(QTimerEvent* event);
public slots:
    void show_log(QString str);
    void test_Motor();
    void UpdateMotorMotionData();
signals:

	void cameraOperate(int);
	void virtualDeviceOpen();
	void virtualDeviceClose();
    void HapticControlRobotStart();
	void HapticControlRobotStop();
	void ForceControlStart();
	void CutFusionControlStart();
	void CutFusionControlStop();
	void ForceThreadStart();
	void ForceThreadStop();
	void HapticReadStart();
	// void surgicalRobotOperate();

private:
    Ui::SurgicalRobotPlatform ui;
	SurgicalRobotConfig* robotConfigSet;
	SurgicalRobotRCM* rcmConfigSet;
    thread_test &thread_pool = thread_test::getInstance(6);
    functiont timerUpdateMotionData;
};
